#include "trajectory_ros.h"
#include <tf2/LinearMath/Quaternion.h>              // 用于tf::getYaw
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>    // 用于createQuaternionMsgFromYaw
#include <tf2/utils.h>


namespace mc{

// 轨迹长度
double TrajectoryROS::calculateLength(Trajectory2D& trajectory) const{
    if (trajectory.size() < 2) return 0.0;
    double length = 0.0;
    for(size_t i = 1; i < trajectory.size(); ++i) {
        length += (trajectory[i].pose.translation.vector() - 
                  trajectory[i-1].pose.translation.vector()).norm();
    }
    return length;    
}

// 计算轨迹曲率
void TrajectoryROS::calculateCurvature(Trajectory2D& trajectory){
    if (trajectory.size() < 3) return;
    
    for (size_t i = 1; i < trajectory.size() - 1; ++i) {
        const auto& p0 = trajectory[i-1].pose.translation.vector();
        const auto& p1 = trajectory[i].pose.translation.vector();
        const auto& p2 = trajectory[i+1].pose.translation.vector();
        
        const double dx1 = p1.x() - p0.x(), dy1 = p1.y() - p0.y();
        const double dx2 = p2.x() - p1.x(), dy2 = p2.y() - p1.y();

        trajectory[i].curvature = (dx1*dy2 - dy1*dx2) / 
            std::pow(dx1*dx1 + dy1*dy1, 1.5);
        
        if (i > 1) {
            const double dt = trajectory[i].time_from_start - trajectory[i-1].time_from_start;
            if (dt > 0) {
                trajectory[i].curvature_rate = 
                    (trajectory[i].curvature - trajectory[i-1].curvature) / dt;
            }
        }
    }
}

// 计算两轨迹点距离
double TrajectoryROS::calculateDistance(TrajectoryPoint2D& a, TrajectoryPoint2D& b) const{
    return (a.pose.translation.vector() - b.pose.translation.vector()).norm();
}

// 计算两轨迹点距离平方
double TrajectoryROS::calculateDistanceSquared(TrajectoryPoint2D& a, TrajectoryPoint2D& b) const{
    return (a.pose.translation.vector() - b.pose.translation.vector()).squaredNorm();
}

// 计算轨迹点a相对于轨迹点b的相对坐标
TrajectoryPoint2D TrajectoryROS::calculateRelativeCoordinate(TrajectoryPoint2D& a, TrajectoryPoint2D& b) const{
    TrajectoryPoint2D pt;
    // 计算相对变换矩阵
    Eigen::Matrix3d Ta = a.pose.toMatrix();
    Eigen::Matrix3d Tb = b.pose.toMatrix();
    Eigen::Matrix3d T_relative = Tb.inverse() * Ta;

    // 从相对变换矩阵中提取平移和旋转信息
    Eigen::Translation2d relativeTranslation(T_relative.block<2, 1>(0, 2));
    Eigen::Rotation2Dd relativeRotation(T_relative.block<2, 2>(0, 0));

    pt.pose =  Pose2D(relativeTranslation.x(), relativeTranslation.y(), relativeRotation.angle());
    return pt;
}

// 与ROS消息互转
Pose2D TrajectoryROS::toPose2D(geometry_msgs::Pose& msg) const{
    return Pose2D(
        msg.position.x,
        msg.position.y,
        tf2::getYaw(msg.orientation)
    );
}

geometry_msgs::Pose TrajectoryROS::toROSPose(Pose2D& pose) const{
    geometry_msgs::Pose ros_pose;
    ros_pose.position.x = pose.translation.x();
    ros_pose.position.y = pose.translation.y();
    tf2::Quaternion q_tf;
    q_tf.setRPY(0.0, 0.0, pose.rotation.angle());
    ros_pose.orientation = tf2::toMsg(q_tf);
    return ros_pose;
}

// Pose2D <-> geometry_msgs::Pose
Pose2D TrajectoryROS::toPose2D(geometry_msgs::PoseStamped& msg) const{
    return toPose2D(msg.pose);
}

geometry_msgs::PoseStamped TrajectoryROS::toROSPoseStamped(Pose2D& pose, string frame_id) const{
    geometry_msgs::PoseStamped ros_pose_stamped;
    ros_pose_stamped.header.frame_id = frame_id;
    ros_pose_stamped.header.stamp = ros::Time::now();
    ros_pose_stamped.pose = toROSPose(pose);
    return ros_pose_stamped;
}

// TrajectoryPoint2D <-> geometry_msgs::Pose
TrajectoryPoint2D TrajectoryROS::toTrajectoryPoint2D(geometry_msgs::Pose& msg, string frame_id) const{
    TrajectoryPoint2D pt;
    pt.pose = toPose2D(msg);
    return pt;
}

geometry_msgs::Pose TrajectoryROS::toROSPose(TrajectoryPoint2D& pt) const{
    return toROSPose(pt.pose);
}

// TrajectoryPoint2D <-> geometry_msgs::PoseStamped
TrajectoryPoint2D TrajectoryROS::toTrajectoryPoint2D(geometry_msgs::PoseStamped& msg) const{
    TrajectoryPoint2D pt;
    pt.pose = toPose2D(msg.pose);
    pt.frame_id = msg.header.frame_id;
    return pt;
}

geometry_msgs::PoseStamped TrajectoryROS::toROSPoseStamped(TrajectoryPoint2D& pt) const{
    return toROSPoseStamped(pt.pose, pt.frame_id);
}

// 消息数据转换
Trajectory2D TrajectoryROS::toTrajectory2D(nav_msgs::Path& path) const{
    Trajectory2D trajectory;
    for(auto& pose_stamped : path.poses) {
        trajectory.push_back(toTrajectoryPoint2D(pose_stamped));
    }
    return trajectory;
}

nav_msgs::Path TrajectoryROS::toROSPath(Trajectory2D& trajectory, string frame_id) const{
    nav_msgs::Path path;
    path.header.frame_id = frame_id;
    path.header.stamp = ros::Time::now();
    for(auto& pt : trajectory) {
        path.poses.push_back(toROSPoseStamped(pt));
    }
    return path;
}


}; // namespace mc